/**
 Message sequence chart for the
 higest level frame and packet exchange
 \msc
 arcgradient = 4;
 a [label = "Host1"],b [label="Target1"],c [label="Target2"],d[label="Host2"];

 a=>b [label = "Frame 0"];
 b=>c [label = "Frame 0"];
 b<=c [label = "lAck"];
 c=>d [label = "Frame 0"];
 c<=d [label = "fAck"];
 b<=c [label = "fAck"];
 b=>c [label = "lAck"];
 a<=b [label = "fAck"];
 \endmsc
 */

#include "project.h"
#include "resources.h"
#include <stdio.h>
#include <util/crc16.h>
#include <assert.h>
#include <stdlib.h>

#define CONSOLE_MODE 1
/**
 * list of mode definitions for the
 * host<->target communuication protocol*/
#define init 		'I'
#define run  		'R'
#define stop 		'S'
#define restart 	'B'	//reBoot
#define quit 		'Q'	//Halt IO and exit task scheduler
#define display 	'D'	//Display a test message
#define testmode 	'E'	//tEstmode
#define packetRCV 	'X'	//RX from network to computer host
#define packetTX 	'T'	//TX to from computer Host to network
#define echo 		'E'
/**definitions for opCodes*/
#define ackPacket 1	//includes crc pass
#define ackLast 9	//crc pass plus len ack
#define nackPacket 0xf0	//
#define crcfail   0xf1
#define LenMismatch 0xf2

/**
 Buffers for sliding window and IO state control\n

 * */
//xQueueHandle packet[10];
//xQueueHandle packetPayload[10];
xQueueHandle ledMode;
xQueueHandle twiTxHeader = NULL, twiTxString = NULL, twiTxCrc = NULL;
xQueueHandle twiRxHeader = NULL, twiRxString = NULL, twiRxCrc = NULL;

/*!
 Semaphores used to flag conditions\n
 or lock resources.
 */
xSemaphoreHandle usartTransmit, usartReceive;    //binary semaphores for usart locking
xSemaphoreHandle usartRxCrcPass, usartRxPktRdy;
//xSemaphoreHandle usartTxPktRdy;
//xSemaphoreHandle twiTxPktRdy;
xSemaphoreHandle twiTxMode = NULL, twiRxPktRdy, twiTxCrcPass;
/**
 Bitfield for the various packet headers defined globally \n
 so that each task can create instances on their respective heaps.
 * */
typedef struct {
	uint8_t src :8;		//source host ID
	uint8_t dst :8;		//dest host ID
	uint8_t lst :1;		//last packet flag   //reverse the middle octet!
	uint8_t rsv :1;		//packet rx'd
	uint8_t nr :3;		//#rec
	uint8_t ns :3;		//# sent
	uint8_t op :8;		//opcode
	uint8_t len :8;		//packet length
} pktHdr;		//type to store packet header parameters
portTASK_FUNCTION(HstTgtFSM_machine,pvParameters) {

//	char * TwiTxStage0=(char*)malloc(7+255+2);
//	char * TwiTxStage1=(char*)malloc(7+255+2);
//	char * usartRxStage0=(char*)malloc(7+255+2);
//	char * usartRxStage1=(char*)malloc(7+255+2);

//	twiTxString = xQueueCreate(90,sizeof(unsigned short));
//	twiTxHeader = xQueueCreate(20,20*sizeof(unsigned char));
	/** Variables for holding packet states and contents
	 *
	 * */
	unsigned char rxPacketHdrbuff[5], rxPayloadBuff[254];
	signed char txPacketHdrbuff[5];
	unsigned short rxlocalCrc = 0x0000, rxLength = 16, rxHostCrc, packetNumber;
	pktHdr rxPacketHdr;
	int rxDone = 0;

	uint16_t crc = 0xffff, checksum = 0;
	int i, j = 0;
	ledMode = xQueueCreate( 5, sizeof(unsigned portBASE_TYPE) );
	twiTxHeader = xQueueCreate(20,5);
	twiTxString = xQueueCreate(20,25);
	twiTxCrc = xQueueCreate(20,2);
//	twiRxHeader = xQueueCreate(10,5);
//	twiRxString = xQueueCreate(10,30);
//	twiRxCrc = xQueueCreate(10,2);

	unsigned char fsmMode, rxModeChar = 0x00;			//, pythonInterface = 0;
	unsigned char displayTemp[300], temp[10];
	vSemaphoreCreateBinary(usartTransmit);
	vSemaphoreCreateBinary(twiTxMode);
//	xSemaphoreTake(twiTxMode,5);
	DDRF = 0xff;
	PORTF = 0xbd;

	Usart_puts("$B<");
	while (1) {

		while (rxModeChar != '$') {
			xSerialGetChar(NULL,&rxModeChar,0);			//read control character
		}
		xSerialGetChar(NULL,&fsmMode,5);
		switch (fsmMode) {
			/**
			 Highest level state machine for controlling the current mode of the\n
			 tranceiver. Modes includes init, run, stop, display and quit.
			 */
			case init:
				Usart_puts("$I<");    //handshake token
				fsmMode = '!';
				break;
			case run:
				Usart_puts("$R<");    //string for python script to confirm run-state
				j = run;
				xQueueSend(ledMode,&j,0);
				fsmMode = '!';
				break;

			case stop: /**-------------CASE STOP---------------*/
				i = stop;
				xQueueSend(ledMode,&i,0);
				Usart_puts("$S<");    //token to indicate stop state
				fsmMode = '!';
				break;
			case quit:
				j = stop;
				Usart_puts("$Q<");    //confirm quit command receipt
				xQueueSend(ledMode,&j,0);    //queuesend takes pointers, #define has no address!
//				Usart_puts("halting Everything");
				vTaskDelay(100);    //delay 10ms to allow queues to clear
				vTaskEndScheduler();    //halt everything
				vTaskSuspendAll();		//Free Heap space and return to first executive loop in main.c
				break;
			case display:
				/**----------------------------*/
				crc = 0x00;
				Usart_puts("$D<");
				i = 0;
				while (((displayTemp[i - 1]) != 0x001b) && (displayTemp[i - 1]) != 0x000d) {    //exit loop on enter and return
					if (xSerialGetChar(NULL,&displayTemp[i],0)) {
//						Usart_putc('D');
//						Usart_putc(displayTemp[i]);
						i++;
					}
				}
				displayTemp[i++] = '\0';
				for (j = 0; j < (strlen(displayTemp) - 1); j++) {    //perform CRC over the length of the input string.
					crc = _crc16_update(crc,displayTemp[j]);	//recursive update to the CRC pattern
					checksum = (displayTemp[j] + checksum) % 0xff;  		  //update the checksum
					//xSerialPrintf("%04X",crc);
				}
				//xSerialPrintf("CRC=0X%04X",crc);  //prints hex formatted CRC value for testing
				for (j = 0; j < (i - 2); j++) {
					Usart_putc(displayTemp[j]);
				}
				xSerialPrintf("0X%04X",crc);
//				Usart_putc((crc & 0xff00) >> 8);
//				Usart_putc(crc & 0x00ff);
				fsmMode = '!';
				break;
				/**----------------------------*/
			case testmode:
				crc = 0x0000;    //initializer value for Xmodem CRC. switch to 0xffff for modbus calc
				//int timerProf = xTaskGetTickCount();	//get current inital tick for timing analysis
				fsmMode = '!';
				break;
			case packetTX:

				///////////////////////////
				/** receive packet case.
				 * general flow:
				 * 		receive 5 chars
				 * 		copy and parse header
				 * 		receive payload
				 * 		check crc
				 * 		xmit ack
				 * 		loop for more packets if necesary
				 * */
				Usart_puts("$T<");
				packetNumber = 0;
				// 5 byte header read
				while (rxDone == 0) {
					rxlocalCrc = 0x0000;
					packetNumber = packetNumber % 8;
					for (i = 0; i < 5;) {    //fetch 5 bytes of header data
						if (xSerialGetChar(NULL,&rxPacketHdrbuff[i],5)) {
							rxlocalCrc = _crc16_update(rxlocalCrc,rxPacketHdrbuff[i++]);    //include the payload in the crc calculation
						}
					}

					if (i != 5) rxPacketHdr.op = LenMismatch;
					rxHostCrc = 0;
					memcpy(&rxPacketHdr,rxPacketHdrbuff,5);    //copy the first five bytes from the buffer to a header struct
					if (rxPacketHdr.lst == 0) {
						rxLength = 16;    //len overflows to zero until last packet.
						rxPacketHdr.op = ackPacket;
					}
					else {
						rxDone = 1;
						rxLength = rxPacketHdr.len;
						rxPacketHdr.op = ackLast;
					}
					//read variable length payload
					for (i = 0; i < rxLength;) {	//read variable length payload
						if (xSerialGetChar(NULL,&rxPayloadBuff[i],20)) {
							rxlocalCrc = _crc16_update(rxlocalCrc,rxPayloadBuff[i++]);    //concurrent crc calculation
						}
					}
					//2 byte crc read

					for (i = 0; i < 2;) {    //read variable length payload
						if (xSerialGetChar(NULL,&temp[i],20)) {
							i++;
						}
					}
					rxHostCrc = string2Ushort(&temp[0]);	//
					Usart_putShort(rxHostCrc);
					rxPacketHdr.ns = packetNumber++;    //send #rx'd packets back to host
					rxPacketHdr.len = rxLength;    //send rx'd payloadLen back

					if (rxlocalCrc == rxHostCrc)
							{

						xQueueSend(twiTxHeader,&rxPacketHdr,10);
						xQueueSend(twiTxString,&rxPayloadBuff,40);    //very problematic
						xQueueSend(twiTxCrc,&rxlocalCrc,10);
						xSemaphoreGive(twiTxMode);
						xSemaphoreTake(twiTxMode,600);
					}
					else {
						rxPacketHdr.op = crcfail;
					}
					i = rxPacketHdr.src;		//set source and dest addresses
					rxPacketHdr.src = rxPacketHdr.dst;
					rxPacketHdr.dst = i;
					memcpy(txPacketHdrbuff,&rxPacketHdr,5);
//					xQueueSend(twiTxHeader,&rxPacketHdr,1);

					for (i = 0; i < 5;) {		//write the outbound packet header
						if (xSerialPutChar(NULL,txPacketHdrbuff[i],15)) {
							i++;
						}
					}

				}
				rxDone = 0;
				fsmMode = '!';
				break;
			case restart:
				fsmMode = init;
				break;
			default:
				break;
		}
		//next case for header info
	}

}
/**USART packet receiver function.
 This task runs continuously but uses\n
 flags set by the main FSM and cleared by \n
 the receivers terminal state to control it's\n
 runstate.

 control input -
 */portTASK_FUNCTION(usartRxPacket,pvParameters) {

//	check semaphore. start recieving packet if flag set.
//	receive 5 bytes
//	copy 5 bytes to location given by of usartPktHdr location
//	get the packet length

}
portTASK_FUNCTION(usartTxPacket,pvParameters) {
	int i = 0;
	while (1) {
		i++;
	}
//	vTaskSet
//	while (1) {
//		if (semaphore_usart_TX==1) {
//			for (i = 0; i < pkt_len; i++) {
//				Usart_putc(usartTxBuf[i]);
//			}
//			xSemaphoreTake(semaphore_usart_TX,0);
//			xSemaphore
//
//		}
//	}
}
portTASK_FUNCTION(twiTxPacket,pvParameters) {
	unsigned char i2cTestString[257];    //This really should be on the heap..
	I2C_Master_Initialise(0x45);
	unsigned int i = 0;
	DDRD = 0xff;
	PORTD = 0xff;
	vTaskDelay(20);    //delay to lockout semaphore

	pktHdr i2cTxHeader;
	while (twiTxMode == NULL ) {
	}

	for (i = 0; i < 255; ++i) {
			PORTB = i << 2;
		}
	while (1) {
		/**Receive packet from the from queue system and pass to
		 * i2c interface. No Flow control at the moment
		 *
		 */
		if (xSemaphoreTake(twiTxMode,20) == pdTRUE) {

			if (twiTxHeader != NULL ) {    //RTOS reboots if this is not yet initialized.
				if (xQueueReceive(twiTxHeader,&i2cTestString[1],20)) {
					memcpy(&i2cTxHeader,&i2cTestString[1],5);
					i2cTestString[0] = (i2cTxHeader.dst << 1) | I2C_WRITE;
					I2C_Master_Start_Transceiver_With_Data(&i2cTestString[0],6);
					I2C_Get_State_Info();
				}
			}

			if (twiTxString != NULL ) {
				if (xQueueReceive(twiTxString,&i2cTestString[1],20)) {
					i2cTestString[0] = (i2cTxHeader.dst << 1) | I2C_WRITE;
					I2C_Master_Start_Transceiver_With_Data(&i2cTestString[0],i2cTxHeader.len + 1);
				}
			}
			if (twiTxCrc != NULL ) {
				if (xQueueReceive(twiTxCrc,&i2cTestString[1],20)) {
					i2cTestString[0] = (i2cTxHeader.dst << 1) | I2C_WRITE;
					I2C_Master_Start_Transceiver_With_Data(&i2cTestString[0],3);
				}
			}
			xSemaphoreGive(twiTxMode);
		}
	}

}

portTASK_FUNCTION(twiRxPacket,pvParameters) {
	int i = run;
	DDRB = 0xff;
	vTaskDelay(10000);
	while (i == run) {

	}
}
portTASK_FUNCTION(ioTask,pvParameters) {
	DDRF = 0xff;
	DDRB = 0xff;
	uint8_t currentLedMode;
	uint16_t i = 0, j = 0;
//	Usart_puts("ioTask Started!");
	while (1) {

		xQueueReceive(ledMode,&currentLedMode,20);
		j = xTaskGetTickCount();
		vTaskDelayUntil(&j,1);
		i = (i + 1) % 100;
		if (currentLedMode == run) {
			if (i == 50) PORTB = 0xff;
			if (i == 1) PORTB = 0x00;
		}
		if (currentLedMode == stop) {
			PORTB = 0x00;    //logic low levels on outputs
			PORTF = 0x00;
			DDRF = 0x00;	//turn off pullup resistors
		}

	}
}
